#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "robo/adc_val.h"
#include "robo/CameraParams.h"
#include <stdlib.h>
//#include "roboard/roboard.h"

using namespace robo;

int adc_interval = 100;
bool sleep_stop = false;

void change_freq(const std_msgs::Int32::ConstPtr& msg)
{
	ROS_INFO("Request to change adc interval to %f", msg->data);
	adc_interval = msg->data;
	sleep_stop = true;
}

int main(int argc, char **argv)
{
/*
    roboio_SetRBVer(RB_100);
    //roboio_SetRBVer(RB_100RD);  // if your RoBoard is RB-100RD
    //roboio_SetRBVer(RB_110);    // if your RoBoard is RB-110
    //roboio_SetRBVer(RB_050);    // if your RoBoard is RB-050

    if (spi_Init(SPICLK_21400KHZ) == false)  // init SPI lib for accessing the A/D converter
    {
        ROS_INFO("ADC ERROR: fail to init SPI lib (%s)!\n", roboio_GetErrMsg());
        return -1;
    }


	if(!adc_InitMCH(ADC_USECHANNEL0+ADC_USECHANNEL1+ADC_USECHANNEL2+ADC_USECHANNEL3+ADC_USECHANNEL4+ADC_USECHANNEL5+ADC_USECHANNEL6+ADC_USECHANNEL7, ADCMODE_RANGE_2VREF,ADCMODE_UNSIGNEDCODING))
	{
		ROS_INFO("ADC ERROR: fail to init MCH(%s)!\n", roboio_GetErrMsg());
		return -1;
	}
	
	int * adc_data;
*/
	ros::init(argc, argv, "roboard_adc");
	ros::NodeHandle n;
	//ros::Publisher adc_pub	= n.advertise<adc_vals>("roboard_adc", 10);
	ros::Subscriber adc_sub	= n.subscribe("roboard_adc_interval", 10, change_freq);
	ros::spin();
	/*
	ros::Rate loop_rate(1000);
	struct timeval start, end;
	while (ros::ok())
	{
		gettimeofday(&start, NULL);
		adc_data=adc_ReadMCH();
		adc_vals raw_data;
		raw_data.val1 = adc_data[0];
		raw_data.val2 = adc_data[1];
		raw_data.val3 = adc_data[2];
		raw_data.val4 = adc_data[3];
		raw_data.val5 = adc_data[4];
		raw_data.val6 = adc_data[5];
		raw_data.val7 = adc_data[6];
		raw_data.val8 = adc_data[7];
		adc_pub.publish(raw_data);
		
		ros::spinOnce();
		gettimeofday(&end, NULL);
		sleep_stop = false;
		while(!sleep_stop && (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < adc_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}
		sleep_stop = false;
		ros::spinOnce();
	}
	adc_CloseMCH();
	*/
	return 0;
}
